/*
 * Quaternion.cpp
 *
 *  Created on: Feb 12, 2010
 *      Author: travis
 */

#include "Quaternion.h"

namespace uav
{
namespace math
{

Quaternion Quaternion::fromAxisAngle(const Vector3 &axis, const Fixed &angle)
{
	Fixed s, c;
	(angle >> 1).sinCos(s, c);
	return Quaternion(c, s * axis);
}

void Quaternion::toAxisAngle(Vector3 &axis, Fixed &angle) const
{
	axis = Vector3(x, y, z);
	Fixed s = axis.length();
	axis /= s;
	angle = Fixed::atan2(s, w);
}

Matrix Quaternion::toMatrix() const
{
	Fixed x2 = x * x;
	Fixed y2 = y * y;
	Fixed z2 = z * z;
	Fixed xy = x * y;
	Fixed xz = x * z;
	Fixed yz = y * z;
	Fixed wx = w * x;
	Fixed wy = w * y;
	Fixed wz = w * z;

	return Matrix(
		Fixed::One - ((y2 + z2) << 1),
		(xy - wz) << 1,
		(xz + wy) << 1,

		(xy + wz) << 1,
		Fixed::One - ((x2 + z2) << 1),
		(yz - wx) << 1,

		(xz - wy) << 1,
		(yz + wx) << 1,
		Fixed::One - ((x2 + y2) << 1)
	);
}

Matrix Quaternion::toMatrixTranspose() const
{
	Fixed x2 = x * x;
	Fixed y2 = y * y;
	Fixed z2 = z * z;
	Fixed xy = x * y;
	Fixed xz = x * z;
	Fixed yz = y * z;
	Fixed wx = w * x;
	Fixed wy = w * y;
	Fixed wz = w * z;

	return Matrix(
		Fixed::One - ((y2 + z2) << 1),
		(xy + wz) << 1,
		(xz - wy) << 1,

		(xy - wz) << 1,
		Fixed::One - ((x2 + z2) << 1),
		(yz + wx) << 1,

		(xz + wy) << 1,
		(yz - wx) << 1,
		Fixed::One - ((x2 + y2) << 1)
	);
}

Quaternion Quaternion::square() const
{
	return Quaternion(
		(w * w) - (x * x + y * y + z * z),
		(w * x) << 1,
		(w * y) << 1,
		(w * z) << 1
	);
}

Quaternion Quaternion::exp() const
{
	Vector3 v(x, y, z);
	Fixed expW = w.exp();
	Fixed s = v.length();
	if (s == Fixed::Zero) return Quaternion(expW, Vector3::Zero);
	Fixed sinS, cosS;
	s.sinCos(sinS, cosS);
	s = (expW * sinS) / s;

	return Quaternion(expW * cosS, s * v);
}

Quaternion Quaternion::log() const
{
	Vector3 v(x, y, z);
	Fixed s = v.length();
	if (s == Fixed::Zero) return Quaternion(w.abs().log(), Vector3::Zero);
	v /= s;

	Fixed r, theta;
	Fixed::toPolar(w, s, r, theta);

	return Quaternion(r.log(), theta * v);
}

Quaternion Quaternion::pow(const Fixed &k) const
{
	return (k * log()).exp();
}

Vector3 Quaternion::rotate(const Vector3 &v) const
{
	Vector3 qxv(
		y * v.z - z * v.y,
		z * v.x - x * v.z,
		x * v.y - y * v.x
	);
	Vector3 qxqxv(
		y * qxv.z - z * qxv.y,
		z * qxv.x - x * qxv.z,
		x * qxv.y - y * qxv.x
	);
	qxv *= w;
	return Vector3(
		v.x + ((qxv.x + qxqxv.x) << 1),
		v.y + ((qxv.y + qxqxv.y) << 1),
		v.z + ((qxv.z + qxqxv.z) << 1)
	);
}

Vector3 Quaternion::rotateInverse(const Vector3 &v) const
{
	Vector3 qxv(
		z * v.y - y * v.z,
		x * v.z - z * v.x,
		y * v.x - x * v.y
	);
	Vector3 qxqxv(
		z * qxv.y - y * qxv.z,
		x * qxv.z - z * qxv.x,
		y * qxv.x - x * qxv.y
	);
	qxv *= w;
	return Vector3(
		v.x + ((qxv.x + qxqxv.x) << 1),
		v.y + ((qxv.y + qxqxv.y) << 1),
		v.z + ((qxv.z + qxqxv.z) << 1)
	);
}

Quaternion Quaternion::operator*(const Quaternion &q) const
{
	return Quaternion(
		w * q.w - x * q.x - y * q.y - z * q.z,
		w * q.x + x * q.w + y * q.z - z * q.y,
		w * q.y - x * q.z + y * q.w + z * q.x,
		w * q.z + x * q.y - y * q.x + z * q.w
	);
}

}}
